// Kevin Kemper

#include <fca_controllers/controller.h>
#include <rtai.h>

/*	INPUT members:

	float			motor_angle;
	float			leg_angle;

	float			motor_velocity;
	float			leg_velocity;

	unsigned char	motor_current;

	unsigned char	command;
*/

#define T2A		((1.0/0.18)*(1.0/17.25))
#define A3		0.05
#define A2		0.1

// spring 1
/*
#define K		125.000000
#define B		1.000000
#define I		0.049118
#define OFFSET	(-0.007351+0.001)
*/

// spring 2
/*
#define K		550.000000
#define B		1.200000
#define I		0.079118
#define OFFSET	(-0.017709+0.208)
*/

// spring 3
#define K		1400.000000
#define B		1.500000
#define I		0.090118
#define OFFSET	(-0.021499+0.207)




extern void initialize_force_controller(ControllerInput *input, ControllerOutput *output, ControllerState *state, 
	ControllerData *data)
{
	output->motor_torque = 0.;
	output->time = 0.0;
	FORCE_CONTROLLER_STATE(state)->time		= 0.0;
	FORCE_CONTROLLER_STATE(state)->trq_m	= 0.0;

	FORCE_CONTROLLER_STATE(state)->xl2_last	= 0.0;
	
	FORCE_CONTROLLER_STATE(state)->xm2_ave	= 0.0;
	FORCE_CONTROLLER_STATE(state)->xl2_ave	= 0.0;
	FORCE_CONTROLLER_STATE(state)->xl3_ave	= 0.0;

}


extern void update_force_controller(ControllerInput *input, ControllerOutput *output, ControllerState *state, ControllerData *data) {

	float spinrg_k		= K;
	float spring_pos	= input->motor_angle	- input->leg_angle + OFFSET;
	float spring_vel	= input->motor_velocity	- input->leg_velocity;
	float spring_force	= spinrg_k * spring_pos;
	
	float torque_cmd = FORCE_CONTROLLER_DATA(data)->torque_cmd;
	
	float torque_force;
	float torque_position;
	
	float t;
	
	float kP	= FORCE_CONTROLLER_DATA(data)->torque_p_gain;
	float kD	= FORCE_CONTROLLER_DATA(data)->torque_d_gain;
	float kF	= FORCE_CONTROLLER_DATA(data)->torque_f_gain;
	
	float trq_m	= FORCE_CONTROLLER_STATE(state)->trq_m;
	
	float e1, e2;
	
	float xm1		= input->motor_angle + OFFSET;
	float xm2		= A2*input->motor_velocity + FORCE_CONTROLLER_STATE(state)->xm2_ave*(1-A2);
	
	float xl1		= input->leg_angle;
	float xl2		= A2*input->leg_velocity + FORCE_CONTROLLER_STATE(state)->xl2_ave*(1-A2);
	float xl2_last	= FORCE_CONTROLLER_STATE(state)->xl2_last;

	float xl3		= (xl2 - xl2_last)/0.001;
	
	FORCE_CONTROLLER_STATE(state)->xl2_last = xl2;
	
	
	xl3		= (xl3*A3)+FORCE_CONTROLLER_STATE(state)->xl3_ave*(1-A3);
	
	
	FORCE_CONTROLLER_STATE(state)->xm2_ave = xm2;
	FORCE_CONTROLLER_STATE(state)->xl2_ave = xl2;
	FORCE_CONTROLLER_STATE(state)->xl3_ave = xl3;
	
	
//	rt_printk( "xl3 %d\n", (int)(xl3));
	
	
	FORCE_CONTROLLER_STATE(state)->time += 0.001;
	output->time = FORCE_CONTROLLER_STATE(state)->time;


	t	= FORCE_CONTROLLER_STATE(state)->time;
	
//	e1 = K*(xm1 - xl1) + B*(xm2 - xl2);
//	e2 = -(K-(B*K)/I-(B*B)/I)*(xm2-xl2) - B*(trq_m/I - xl3);
	
	e1 = K*(xm1 - xl1);
	e2 = B*(xm2 - xl2);
	
	// force controller
	torque_force	= 	kP*e1 + kD*e2 - kF*xl3*I;


	if (torque_force > FORCE_CONTROLLER_DATA(data)->trq_lim) {
		torque_force =  FORCE_CONTROLLER_DATA(data)->trq_lim;
	}
	else if (torque_force < -FORCE_CONTROLLER_DATA(data)->trq_lim) {
		torque_force = -FORCE_CONTROLLER_DATA(data)->trq_lim;
	}
	
	FORCE_CONTROLLER_STATE(state)->trq_m = torque_force;


	// position controller
	torque_position	= 	- FORCE_CONTROLLER_DATA(data)->pos_p_gain * (FORCE_CONTROLLER_DATA(data)->pos_cmd - input->motor_angle)
								+ FORCE_CONTROLLER_DATA(data)->pos_d_gain * input->motor_velocity;
	
	
	
	output->motor_torque = T2A*((FORCE_CONTROLLER_DATA(data)->tp_ratio * torque_force) + ((1.0-FORCE_CONTROLLER_DATA(data)->tp_ratio) * torque_position));
	
}


extern void takedown_force_controller(ControllerInput *input, ControllerOutput *output, ControllerState *state, 
	ControllerData *data)
{
	output->motor_torque = 0.;
	output->time = 0.0;
	FORCE_CONTROLLER_STATE(state)->time = 0.0;
}
